/*************************************************************
*
* M3D_VECT.CPP
*
* Vectors / points mathematics (2D / 3D)
* All angles are represented in degrees!!!
*************************************************************/
#include <stdafx.h>

#include <stdlib.h>

//#include <ap_comm.h>
#include <m3d\m3d.h>

//
// Global variables
//
m3dV m3dVUnitX     = {1.f, 0.f, 0.f};
m3dV m3dVUnitY     = {0.f, 1.f, 0.f};
m3dV m3dVUnitZ     = {0.f, 0.f, 1.f};
m3dV m3dVZero      = {0.f, 0.f, 0.f};
m3dV m3dVOneXYZ    = {1.f, 1.f, 1.f};
m3dV m3dVUndef     = {M3D_FLOAT_UNDEF, M3D_FLOAT_UNDEF, M3D_FLOAT_UNDEF};
m3dV m3dVInfinity  = {M3D_INFINITY, M3D_INFINITY, M3D_INFINITY};

m3dV m3dVUnitXNeg = {-1.f, 0.f, 0.f};
m3dV m3dVUnitYNeg = {0.f, -1.f, 0.f};
m3dV m3dVUnitZNeg = {0.f, 0.f, -1.f};

m2dV m2dVUnitX  = {1.f, 0.f};
m2dV m2dVUnitY  = {0.f, 1.f};
m2dV m2dVZero   = {0.f, 0.f};
m2dV m2dVOneXY  = {1.f, 1.f};
m2dV m2dVUndef  = {M3D_FLOAT_UNDEF, M3D_FLOAT_UNDEF};

m3dVINT m3dVIntZero = {0, 0, 0};


/*************************************************************
* m3dNormalizeVertList()
*
*************************************************************/
void m3dNormalizeVertList (int nVert, m3dV *vertList)
{
   if (nVert <= 0) {
      return;
   }

#if defined M3D_USE_SSE
   static const float three = 3.0f;
   static const float half = 0.5f;

   __asm {
      mov         eax, vertList
      mov         ecx, nVert
      movss       xmm7, M3D_EPSILON_2
   _sse_loop:
      prefetchnta [eax + 32]
      movss       xmm0, dword ptr [eax]
      movss       xmm6, dword ptr [eax+4]
      movss       xmm5, dword ptr [eax+8]
      movaps      xmm2, xmm0
      mulss       xmm2, xmm2
      movaps      xmm3, xmm6
      mulss       xmm3, xmm3
      addss       xmm2, xmm3
      movaps      xmm3, xmm5
      mulss       xmm3, xmm3
      movaps      xmm4, xmm3
      movaps      xmm3, xmm2
      addss       xmm3, xmm4
      comiss      xmm3, xmm7
      jb short    _sse_err
      movss       xmm1, three                // x1 = 3
      movss       xmm2, half                 // x2 = .5
      rsqrtss     xmm4, xmm3                 // x4 = apr(1 / sqrt(len))
      mulss       xmm3, xmm4                 // m3 = apr(sqrt(len))
      mulss       xmm2, xmm4                 // m2 = apr(1 / sqrt(len)) / 2
      mulss       xmm3, xmm4                 // m3 = apr(1)
      subss       xmm1, xmm3                 // m1 = 3  - apr(1)
      mulss       xmm2, xmm1                 // m2 = apr(1 / sqrt(len)) / 2 * (3 - apr(1))
      mulss       xmm0, xmm2
      mulss       xmm6, xmm2
      movss       dword ptr [eax+0],xmm0
      mulss       xmm5, xmm2
      movss       dword ptr [eax+4],xmm6
      movss       dword ptr [eax+8],xmm5

      add         eax, 12
      dec         ecx
      jne short   _sse_loop
      jmp short   _sse_exit

   _sse_err:                                 // vector.y = 1.0f is faster than vector = unitY
      mov         [eax+4], 0x3f800000        // 1.0f
      add         eax, 12
      dec         ecx
      jne short   _sse_loop
   _sse_exit:
   }

#else
   int   i;
   for (i = 0; i < nVert; i++) {
      m3dNormalize(&vertList[i]);
   }
#endif
}

/*************************************************************
* m3dNormalizeVertListFast()
*
*************************************************************/
void m3dNormalizeVertListFast (int nVert, m3dV *vertList)
{
   if (nVert <= 0) {
      return;
   }

#if defined M3D_USE_SSE
   __asm {
      mov         eax, vertList
      mov         ecx, nVert
   _sse_loop:
      prefetchnta [eax + 32]
      movss       xmm0, dword ptr [eax]
      movss       xmm6, dword ptr [eax+4]
      movss       xmm5, dword ptr [eax+8]
      movaps      xmm2, xmm0
      mulss       xmm2, xmm2
      movaps      xmm3, xmm6
      mulss       xmm3, xmm3
      addss       xmm2, xmm3
      movaps      xmm3, xmm5
      mulss       xmm3, xmm3
      movaps      xmm4, xmm3
      movaps      xmm3, xmm2
      addss       xmm3, xmm4                 // x3 = len =  x*x + y*y + z*z
      rsqrtss     xmm3, xmm3                 // x4 = apr(1 / sqrt(len))
      mulss       xmm0, xmm3
      mulss       xmm6, xmm3
      movss       dword ptr [eax+0],xmm0
      mulss       xmm5, xmm3
      movss       dword ptr [eax+4],xmm6
      movss       dword ptr [eax+8],xmm5
      add         eax, 12
      dec         ecx
      jne short   _sse_loop
   }

#else
   int   i;
   for (i = 0; i < nVert; i++) {
      _m3dNormalize(&vertList[i]);
   }
#endif
}

/*************************************************************
* _m3dAngleVector_COS()
*
* IN : v1, v2 - NORMALIZED vectors in 3D
* RET: cosine of angle between two vectors in 3D
*************************************************************/
float _m3dAngleVector_COS (const m3dV *v1, const m3dV *v2)
{
   float cosangle;
   
   cosangle = m3dDotProduct(v1,v2);

   // because of the extra bits in FPU, it appears eventually
   // that computed 1.f is actualy > than 1.f (constant)
   return _m3dClamp(-1.f, 1.f, cosangle);
}
/*************************************************************
* _m3dAngleVector()
*
* IN : v1, v2 - NORMALIZED vectors in 3D
* RET: angle [0..180] between 2 vectors in 3D
*
* NOTE: Angle is UNSIGNED. m3xzAngleXXX(), etc. - 
*       these functions return signed vectors
*************************************************************/
float _m3dAngleVector (const m3dV *v1, const m3dV *v2)
{
   float cosangle;

   cosangle = _m3dAngleVector_COS(v1,v2);
   
   return (float)m3dRad2Deg(m3dACos(cosangle));
}

/*************************************************************
* m3dAngleVector_COS()
*
* IN : v1, v2 - vectors in 3D
* RET: cosine of angle between two vectors in 3D
*************************************************************/
float m3dAngleVector_COS (const m3dV *v1, const m3dV *v2)
{
   float cosangle, len;
   
   len = m3dLengthVector(v1)*m3dLengthVector(v2);
   if (m3dIsZero(len)) {
      // angle = 0, cosAngle = 1
      return 1.f;
   }
   cosangle = m3dDotProduct(v1,v2) / len;

   // because of the extra bits in FPU, it appears eventually
   // that computed 1.f is actually > than 1.f (constant)
   return _m3dClamp(-1.f, 1.f, cosangle);
}

/*************************************************************
* m3dAngleVector()
*
* IN : v1, v2 - vectors in 3D
* RET: angle [0..180] between 2 vectors in 3D
*
* NOTE: Angle is UNSIGNED. m3xzAngleXXX(), etc. - 
*       these functions return signed vectors
*************************************************************/
float m3dAngleVector (const m3dV *v1, const m3dV *v2)
{
   float cosangle;

   cosangle = m3dAngleVector_COS(v1,v2);
   
   return (float)m3dRad2Deg(m3dACos(cosangle));
}

/*************************************************************
* m3dAngleVectorAxis()
*
*************************************************************/
float m3dAngleVectorAxis(const m3dV *axis, const m3dV *v1, const m3dV *v2)
{
   float angle = m3dAngleVector(v1, v2);
   
   m3dV  ax;
   m3dCrossProduct(v1, v2, &ax);

   if (m3dDotProduct(&ax, axis) < 0.0f) {
      angle = -angle;
   }

   return angle;
}

/*************************************************************
* m3dAngleVectorAxis()
*  
*  [-180... 180]
*
* norm - unit vector
* angle between projections of v1 and v2 on plane with normal norm
*************************************************************/
float  m3dAngleVector      (const m3dV *v1, const m3dV *v2, const m3dV* norm)
{
   ASSERT(m3dIsEqual_A(m3dLengthVector(norm), 1.f, 2*M3D_EPSILON));

   m3dV _v1 = (*v1) - m3dDotProduct(v1, norm)*(*norm);
   m3dV _v2 = (*v2) - m3dDotProduct(v2, norm)*(*norm);

   ASSERT((m3dLengthVector(&_v1) > 0.01f) && (m3dLengthVector(&_v2) > 0.01f));

   float angle = m3dAngleVector(&_v1, &_v2);

   m3dV crp;
   m3dCrossProduct(&_v1, &_v2, &crp);

   if (m3dDotProduct(&crp, norm) < 0.f) {
      angle = -angle;
   }

   return angle;
}

/*************************************************************
* m3xzAngleVector()
*
* IN : v1, v2 - two vectors in R3XZ
* OUT: angle in degrees [-180..180] between them in XZ projection
*************************************************************/
float m3xzAngleVector (const m3dV *_v1, const m3dV *_v2)
{
   float cosangle, angle;
   m3dV  crossProd;
   m3dV  v1(*_v1), v2(*_v2);

   // drop Y-coordinate
   v1.y = 0.f;
   v2.y = 0.f;

   cosangle = m3dAngleVector_COS(&v1, &v2);

   // check whether it's calculated correctly
   if (cosangle < -1.f || cosangle > 1.f)  {
      ASSERT(FALSE);
   }

   angle = m3dACos(cosangle);
   angle = m3dRad2Deg(angle);

   // this stupid code is intended to fix up the bug in cos()/acos()
   // functions - they return values slightly more/less then -1.0/1.0
   if (angle < 0.f)  {
      ASSERT(angle > -50.f*M3D_EPSILON);
      angle = 0.f;
   }

   if (angle > 180.f)  {
      ASSERT(angle < 180.f + 50.f*M3D_EPSILON);
      angle = 180.f;
   }

   ASSERT(angle >= 0.f && angle <= 180.f);

   // take care of angle orientation
   m3dCrossProduct(&v1, &v2, &crossProd);   
   
   if (crossProd.y < 0.f)  {
      angle = - angle;
   }

   // check the supposed range of the obtained angle
   if (angle < -180.f || angle > 180.f)  {
      ASSERT(FALSE);
   }

   return angle;
}

/*************************************************************
* m3xzAngleVector_COS()
*
* IN : v1, v2 - vectors in 3D
* RET: cosine of angle between them in XZ projection
*************************************************************/
float m3xzAngleVector_COS (const m3dV *v1, const m3dV *v2)
{
   float cosangle, len2;

   len2 = (v1->x * v1->x + v1->z * v1->z)*
          (v2->x * v2->x + v2->z * v2->z);
   if (m3dIsZero(len2)) {
      // angle = 0, cosAngle = 1
      return 1.f;
   }
   cosangle = (v1->x * v2->x + v1->z * v2->z) / m3dSqrt(len2);

   // because of the extra bits in FPU, it appears eventually
   // that computed 1.f is actualy > than 1.f (constant)
   if (cosangle < -1.f)  {
      cosangle = -1.f;
   }

   if (cosangle > 1.f)  {
      cosangle = 1.f;
   }

   return cosangle;
}

/*************************************************************
* m3xzAngleVector()
*
* IN : v1, v2 - two vectors in R3XY
* OUT: angle in degrees [-180..180] between them in XY projection
*************************************************************/
float m3xyAngleVector (const m3dV *_v1, const m3dV *_v2)
{
   float cosangle, angle;
   m3dV  crossProd;
   m3dV v1(*_v1), v2(*_v2);

   // drop Z-coordinate
   v1.z = 0.f;
   v2.z = 0.f;

   cosangle = m3dAngleVector_COS(&v1, &v2);

   // check whether it's calculated correctly
   if (cosangle < -1.f || cosangle > 1.f)  {
      ASSERT(FALSE);
   }

   angle = m3dACos(cosangle);
   angle = m3dRad2Deg(angle);

   // this stupid code is intended to fix up the bug in cos()/acos()
   // functions - they return values slightly more/less then -1.0/1.0
   if (angle < 0.f)  {
      ASSERT(angle > -50.f*M3D_EPSILON);
      angle = 0.f;
   }

   if (angle > 180.f)  {
      ASSERT(angle < 180.f + 50.f*M3D_EPSILON);
      angle = 180.f;
   }

   ASSERT(angle >= 0.f && angle <= 180.f);

   // take care of angle orientation
   m3dCrossProduct(&v1, &v2, &crossProd);   
   
   if (crossProd.z < 0.f)  {
      angle = - angle;
   }

   // check the supposed range of the obtained angle
   if (angle < -180.f || angle > 180.f)  {
      ASSERT(FALSE);
   }

   return angle;
}

/*************************************************************
* m3xyAngleVector_COS()
*
* IN : v1, v2 - vectors in 3DXY
* RET: cosine of angle between them in XY projection
*************************************************************/
float m3xyAngleVector_COS (const m3dV *v1, const m3dV *v2)
{
   float angle, cosangle;

   angle = m3xyAngleVector(v1,v2);

   cosangle = angle;

   // because of the extra bits in FPU, it appears eventually
   // that computed 1.f is actualy > than 1.f (constant)
   if (cosangle < -1.f)  {
      cosangle = -1.f;
   }

   if (cosangle > 1.f)  {
      cosangle = 1.f;
   }

   return cosangle;
}

/*************************************************************
* m3yzAngleVector()
*
* IN : v1, v2 - two vectors in R3YZ
* OUT: angle in degrees [-180..180] between them in YZ projection
*************************************************************/
float m3yzAngleVector (const m3dV *_v1, const m3dV *_v2)
{
   float cosangle, angle;
   m3dV  crossProd;
   m3dV v1(*_v1), v2(*_v2);

   // drop X-coordinate
   v1.x = 0.f;
   v2.x = 0.f;

   cosangle = m3dAngleVector_COS(&v1, &v2);

   // check whether it's calculated correctly
   if (cosangle < -1.f || cosangle > 1.f)  {
      ASSERT(FALSE);
   }

   angle = m3dACos(cosangle);
   angle = m3dRad2Deg(angle);

   // this stupid code is intended to fix up the bug in cos()/acos()
   // functions - they return values slightly more/less then -1.0/1.0
   if (angle < 0.f)  {
      ASSERT(angle > -50.f*M3D_EPSILON);
      angle = 0.f;
   }

   if (angle > 180.f)  {
      ASSERT(angle < 180.f + 50.f*M3D_EPSILON);
      angle = 180.f;
   }

   ASSERT(angle >= 0.f && angle <= 180.f);

   // take care of angle orientation
   m3dCrossProduct(&v1, &v2, &crossProd);   
   
   if (crossProd.x < 0.f)  {
      angle = - angle;
   }

   // check the supposed range of the retrieved angle
   if (angle < -180.f || angle > 180.f)  {
      ASSERT(FALSE);
   }

   return angle;
}

/*************************************************************
* m3yzAngleVector_COS()
*
* IN : v1, v2 - vectors in 3DYZ
* RET: cosine of angle between them in YZ projection
*************************************************************/
float m3yzAngleVector_COS (const m3dV *v1, const m3dV *v2)
{
   float angle, cosangle;

   angle = m3yzAngleVector(v1,v2);

   cosangle = angle;

   // because of the extra bits in FPU, it appears eventually
   // that computed 1.f is actualy > than 1.f (constant)
   if (cosangle < -1.f)  {
      cosangle = -1.f;
   }

   if (cosangle > 1.f)  {
      cosangle = 1.f;
   }

   return cosangle;
}

/*************************************************************
* m2dBasisCoord()
*
* Calculate vector coords in a given basis (va, vb)
* The linear independancy of (va, vb) is checked via ASSERT
*
* IN: va, vb - basis vectors
*     v      - vector for decomposition
* OUT:ka, kb - decomposition coefficients    
*************************************************************/
void m2dBasisCoord (m2dV *va, m2dV *vb, m2dV *v, float *ka, float *kb)
{
   float d   = va->x*vb->y - va->y*vb->x;
   float ood = 1.f / d; 
   ASSERT(d > 10.f*M3D_EPSILON || d < -10.f*M3D_EPSILON);

   *ka = (v->x*vb->y - v->y*vb->x) * ood;
   *kb = (v->y*va->x - v->x*va->y) * ood;

   return;
}

/*************************************************************
* m3xzBasisCoord()
*
* Calculate vector coords in a given basis (va, vb)
* The linear independancy of (va, vb) is checked via ASSERT
*
* IN: va, vb - basis vectors
*     v      - vector for decomposition
* OUT:ka, kb - decomposition coefficients    
*************************************************************/
void m3xzBasisCoord (m3dV *va, m3dV *vb, m3dV *v, float *ka, float *kb)
{
   float d;

   d = va->x*vb->z - va->z*vb->x;

   ASSERT(d > 10.f*M3D_EPSILON || d < -10.f*M3D_EPSILON);

   *ka = (v->x*vb->z - v->z*vb->x)/d;
   *kb = (v->z*va->x - v->x*va->z)/d;

   return;
}

/*************************************************************
* m3dBasisCoord()
*
* Get vector coords in a given basis (va, vb)
* The linear independancy of (va, vb) is checked through ASSERT
* The result is obtained via the solution of the system of linear 
* equations. The system is redundant; if one Det is zero, find another
*
* IN: va, vb - basis vectors
*     v      - vector for decomposition
* OUT:ka, kb - decomposition coefficients    
*************************************************************/
void m3dBasisCoord (m3dV *va, m3dV *vb, m3dV *v, float *ka, float *kb)
{
   float d1, d2, d3;
   int   maxD;

   d1 = va->x*vb->z - va->z*vb->x;
   d2 = va->x*vb->y - va->y*vb->x;
   d3 = va->z*vb->y - va->y*vb->z;

   if (m3dFAbs(d1) < 0.1f*M3D_EPSILON &&
       m3dFAbs(d2) < 0.1f*M3D_EPSILON &&
       m3dFAbs(d3) < 0.1f*M3D_EPSILON) {
      // vectors are collinear
      float    t;
#ifndef _RELEASE
      m3dV  p;
      m3dCrossProduct(va, v, &p);
      ASSERT(m3dLengthVector(&p) < 0.1f*M3D_EPSILON); // make sure "v" is collinear with "va"/"vb"
#endif
      t = m3dLengthVector(va);
      if (t > 0.1f*M3D_EPSILON) {
         *ka = m3dDotProduct(va, v) / t;
         *kb = 0.f;
      } else {
         t = m3dLengthVector(vb);
         if (t > 0.1f*M3D_EPSILON) {
            *kb = m3dDotProduct(vb, v) / t;
            *ka = 0.f;
         } else {
            ASSERT(FALSE);
            *ka = *kb = 0.f;
         }
      }
      return;
   }
   if (m3dFAbs(d1) > m3dFAbs(d2))  {
      if (m3dFAbs(d1) > m3dFAbs(d3))  {
         maxD = 1;
      } else {
         maxD = 3;
      }
   } else {
      if (m3dFAbs(d2) > m3dFAbs(d3))  {
         maxD = 2;
      } else {
         maxD = 3;
      }
   }

   if (maxD == 1)  {
      ASSERT(m3dFAbs(d1) > 0.1f*M3D_EPSILON);
      *ka = (v->x*vb->z - v->z*vb->x)/d1;
      *kb = (v->z*va->x - v->x*va->z)/d1;
   } else if (maxD == 2)  {
      ASSERT(m3dFAbs(d2) > 0.1f*M3D_EPSILON);
      *ka = (v->x*vb->y - v->y*vb->x)/d2;
      *kb = (v->y*va->x - v->x*va->y)/d2;
   } else {
      ASSERT(m3dFAbs(d3) > 0.1f*M3D_EPSILON);
      *ka = (v->z*vb->y - v->y*vb->z)/d3;
      *kb = (v->y*va->z - v->z*va->y)/d3;
   }

   return;
}

/*************************************************************
* m2dAngleVector_COS()
*
* IN : v1, v2 - vectors in 2D
* RET: cosine of angle between them 
*************************************************************/
float m2dAngleVector_COS (const m2dV *v1, const m2dV *v2)
{
   float cosangle = m2dDotProduct(v1,v2) / (m2dLengthVector(v1)*m2dLengthVector(v2));

   // because of the extra bits in FPU, it appears eventually
   // that computed 1.f is actualy > than 1.f (constant)
   if (cosangle < -1.f)  {
      cosangle = -1.f;
   }

   if (cosangle > 1.f)  {
      cosangle = 1.f;
   }

   return cosangle;
}

/*************************************************************
* m2dAngleVector()
*
* IN : v1, v2 - two vectors in R3XZ
* RET: angle in degrees [-180..180] between the vectors
*************************************************************/
float m2dAngleVector (const m2dV *v1, const m2dV *v2)
{
   m3dV  _v1, _v2;

   m3xzV2DToV3D(v1,&_v1);
   m3xzV2DToV3D(v2,&_v2);

   return m3xzAngleVector(&_v1, &_v2);
}
/******************************************************
* m3dAngleVectorRay_S ()
*
* computes angle between two points around ray
* IN  : pRayOrg - pointer to ray origin
*       pRayDir - pointer to ray direction
*       pPoint1 - pointer to first pointer
*       pPoint2 - pointer to first pointer
* RET: angle with sign between points
******************************************************/
float m3dAngleVectorRay_S (const m3dV *rayOrg, const m3dV *rayDir, const m3dV *pV1, const m3dV *pV2)
{
   m3dV v1, v2;
   m3dV base1, base2;

   // move points into ray origin LCS
   m3dSubtractVector(pV1, rayOrg, &v1);
   m3dSubtractVector(pV2, rayOrg, &v2);

   // compute basis
   m3dTangentComponent(&v1, rayDir, &base1);
   m3dNormalize(&base1);
   m3dCrossProduct(rayDir, &base1, &base2);

   // compute angle
   return m3dRad2Deg(m3dATan2(m3dDotProduct(&v2, &base2), m3dDotProduct(&v2, &base1)));
}

/*************************************************************
* m3dGetTangent ()
*
*************************************************************/
void m3dGetTangent (m3dV *norm, m3dV *tang)
{
   m3dV   vyNC;

   if ((m3dFAbs(norm->x) < M3D_EPSILON) && (m3dFAbs(norm->z) < M3D_EPSILON))   {
      // then vz has direction of oY axis
      vyNC = m3dVUnitX;
   }  else   {
      vyNC = m3dVUnitY;
   }
   m3dTangentComponent(&vyNC, norm, tang);
   m3dNormalize(tang);
   return;
}

/**********************************************************************
* m3dGetPerpVector()
*
* IN  : v - 3D vector
* OUT : vector orthogonal to v with the same length
**********************************************************************/
void m3dGetPerpVector(m3dV const*v, m3dV *vPerp)
{
   m3dV _v = *v;

   if (!_m3dNormalize(&_v)) {
      ASSERT(FALSE);
      *vPerp = m3dVZero;
      return;
   }
   if (m3dFAbs(_v.x) < 0.8f) {
      m3dMakeVector(vPerp, 0.f, -_v.z, _v.y);
   } else {
      m3dMakeVector(vPerp, -_v.y, _v.x, 0.f);
   }
   return;
}

/**********************************************************************
* m2dGetPerpVector()
*
* IN  : v - 2D vector
* OUT : vector orthogonal to v with the same length
**********************************************************************/
void m2dGetPerpVector(m2dV const*v, m2dV *vPerp)
{
   m2dV _v = *v;

   if (!m2dNormalize(&_v)) {
      ASSERT(FALSE);
      vPerp->x = vPerp->y = 0.f;
      return;
   }
   m2dMakeVector(vPerp, -_v.y, _v.x);
   return;
}
/***************************************************************
* m3dMirrorPointPlane()
*   Mirror point by plane
* IN : planePoint - any point in plane
*      planeNorm  - plane normal
*      point      - point to mirror
* OUT: mirror     - result of mirroring
* RET: none
***************************************************************/
void m3dMirrorPointPlane (m3dV *planePoint, m3dV *planeNorm, m3dV *point, m3dV *mirror) 
{
   m3dV  pointShift, pointNorm;

   m3dSubtractVector(point, planePoint, &pointShift);

   m3dNormalComponent(&pointShift, planeNorm, &pointNorm);

   _m3dLineCombine(&pointShift, &pointNorm, -2.f, mirror);
   _m3dAddVector(mirror, planePoint);
   
   return;
}

/***************************************************************
* m3dReflectVector()
*
***************************************************************/
void m3dReflectVector (m3dV *vNormal, m3dV *vIncident, m3dV *vReflect)
{
   float prod;

   prod = m3dDotProduct(vIncident, vNormal);
   _m3dLineCombine(vIncident, vNormal, -2.f * prod, vReflect);
   return;
}

/***************************************************************
* m3dWCS2SphereCoord()
*
* Get spherical coordinates
* OUT: rho - 0..PI (vertical, 0 means vY)
*      phi - 0..2*PI (horizonal, 0 means vX)
***************************************************************/
void m3dWCS2SphereCoord (m3dV *v, float *rho, float *phi)
{
   float phi_cos;

   ASSERT(m3dIsZero(m3dLengthVector(v) - 1.f));

   *rho = m3dACos(v->y);
   phi_cos = (float)(v->x / m3dCos(*rho));
   if (phi_cos < -0.9999f) {
      *phi = M3D_PI;
   } else if (phi_cos > 0.9999f) {
      *phi = 0.f;
   } else {
      if (v->z >= 0.f) {
         *phi = m3dACos(phi_cos);
      } else {
         *phi = 2*M3D_PI-m3dACos(phi_cos);
      }
   }
   return;
}

/***************************************************************
* m3dSphereCoord2WCS()
*
* Get spherical coordinates
* IN: radius - ...
*     rho    - 0..PI (vertical, 0 means vY)
*     phi    - 0..2*PI (horizonal, 0 means vX)
*
***************************************************************/
void m3dSphereCoord2WCS (float radius, float rho, float phi, m3dV *v)
{
   ASSERT(rho >= 0.f && rho <= M3D_PI);
   ASSERT(phi >= 0.f && phi <= 2.f*M3D_PI);
   ASSERT(radius > 0.f);

   v->y = m3dCos(rho);
   v->x = m3dSin(rho)*m3dCos(phi);
   if (1.f - v->x*v->x - v->y*v->y < M3D_EPSILON) {
      ASSERT(1.f - v->x*v->x - v->y*v->y > -0.01f);
      v->z = 0.f;
   } else {
      v->z = (float)m3dSqrt(1.f - v->x*v->x - v->y*v->y);
   }
   if (phi > M3D_PI) {
      v->z = -v->z;
   }

   float r, p;
   m3dWCS2SphereCoord(v, &r, &p);
   ASSERT(m3dIsZero_A(r-rho, 0.1f));
   ASSERT(m3dIsZero_A(p-phi, 0.1f));

   _m3dScaleVector(v, radius);
   return;
}

void m3dSphereClip(m3dV *vSrc, float maxLen)
{
   if ( m3dLengthVector(vSrc) > maxLen ) {
      m3dNormalize(vSrc);
      *vSrc *= maxLen;
   }
}

void m3dBoxClip(m3dV *vSrc, float maxComponent)
{
   vSrc->x = _m3dClamp(-maxComponent, maxComponent, vSrc->x);
   vSrc->y = _m3dClamp(-maxComponent, maxComponent, vSrc->y);
   vSrc->z = _m3dClamp(-maxComponent, maxComponent, vSrc->z);
}

m3dV m3dSphereClippedCopy(const m3dV *vSrc, float maxLen)
{
   m3dV tmp = *vSrc;
   m3dSphereClip(&tmp, maxLen);
   return tmp;
}

m3dV m3dBoxClippedCopy(const m3dV *vSrc, float maxLen)
{
   m3dV tmp = *vSrc;
   m3dBoxClip(&tmp, maxLen);
   return tmp;
}


//
// End-of-file 'M3D_VECT.CPP'
//
